#include "rclcpp/rclcpp.hpp"
#include <mutex>
#include <agv_srvs/srv/outputs_new.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_msgs/msg/periph_teleop.hpp>
#include <agv_msgs/msg/pair_type_int.hpp>
#include <std_msgs/msg/time.hpp>

// 急停
#define EMG_BUTTON "emergencyButton"
#define EMG_BUTTON_2 "emergencyButton2"
// 举升
#define LIFT_F "liftForward"
#define LIFT_B "liftBackward"


class manual_lift
{
private:
    
    rclcpp::Node node_handle;
    ros::Subscriber periTelopSub;
    ros::ServiceClient outClient;
    ros::Subscriber subIoNew;
    bool stop;
    struct OpCode
    {
        int code;
        std_msgs::msg::Time time;
    };

    OpCode op_code;
    std::mutex m_opMtx;
    std::vector<agv_msgs::msg::PairTypeInt> contro_io;

public:
    manual_lift(const rclcpp::Node& nh);
    ~manual_lift();
    bool init();
    void update();
    void readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew &msg);
    void periphTeleopCtlCB(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg);

};

manual_lift::manual_lift(const rclcpp::Node& nh) : node_handle(nh),
                                                      stop(false)
{
    agv_msgs::msg::PairTypeInt singleIo;
    singleIo.value = 0;
    singleIo.key = LIFT_F;
    contro_io.push_back(singleIo);
    singleIo.key = LIFT_B;
    contro_io.push_back(singleIo);

    for (size_t i = 0; i < contro_io.size(); i++)
    {
        ROS_INFO_STREAM("key:" << contro_io[i].key << " value:" << contro_io[i].value);
    }

}

manual_lift::~manual_lift() {}

bool manual_lift::init()
{
    periTelopSub = node_handle.subscribe("/base/periphTeleopInfo", 1, &manual_lift::periphTeleopCtlCB, this);
    outClient = node_handle.serviceClient<agv_srvs::srv::OutputsNew>("/outputNew");
    subIoNew = node_handle.subscribe("/readInputNew", 5, &manual_lift::readInputNewCallBack, this);
}

void manual_lift::update()
{
    std::lock_guard<std::mutex> locker(m_opMtx);
    agv_srvs::srv::OutputsNew srv;

    ROS_INFO_STREAM("code:" << op_code.code << " time:" << op_code.time.data << " now time:" << rclcpp::Time::now());
    if (rclcpp::Time::now() - op_code.time.data > rclcpp::Duration(0.2))
    {
        contro_io[0].value = 0;
        contro_io[1].value = 0;
    } else {
        if (op_code.code == 3) {
            contro_io[0].value = 1;
            contro_io[1].value = 0;
            ROS_INFO_STREAM("io lift forward");
        } else if (op_code.code == 4) {
            contro_io[0].value = 0;
            contro_io[1].value = 1;
            ROS_INFO_STREAM("io lift backward");
        }
    }

    if (stop) {
        contro_io[0].value = 0;
        contro_io[1].value = 0;
    }

    srv.request.array = contro_io;
    if (outClient.call(srv) && srv.response.state) 
    {
        ROS_INFO_STREAM("io lift set success;");
    } else {
        ROS_ERROR_STREAM("io lift set failed;");
    }
}

void manual_lift::readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew &msg)
{
    for (size_t j = 0; j < msg.array.size(); j++)
    {
        auto io_msg = msg.array[j];
        if (io_msg.key == EMG_BUTTON || io_msg.key == EMG_BUTTON_2) {
            if (io_msg.value == 1) {
                ROS_INFO_STREAM("emergency button is preesed");
                stop = true;
            } else {
                stop = false;
            }
        }
    }
}

void manual_lift::periphTeleopCtlCB(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg)
{
    op_code.code = msg->order;
    op_code.time.data = rclcpp::Time::now();

    uint32_t freq = msg->freq;
    if (!freq) {
        ROS_INFO_STREAM("periphTeleopCtlCB: freg is not valid: freq=" << freq);
        return;
    }
}

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = rclcpp::Node::make_shared("manual_peripheral_node");

    manual_lift lift(node);
    lift.init();

    rclcpp::Rate rate(50);
    while (rclcpp::ok())
    {
        lift.update();

        rclcpp::spin_some(node);
		rate.sleep();
    };
	
	return 0;
}